#include "pid.h"

uint8_t PID_apply(__code PID_Param* p, PID_State* s, int16_t err)
{
    s->dea += U16xI16_RL16(p->alpha, err-s->err_);
    if(!p->func(U16xI16_RL16(p->Kp, err)+
                U16xI16_RL16(p->Ki, s->inte)+
                U16xI16_RL16(p->Kd, s->dea))){
        s->inte += err;
        return 0;
    }
    s->err_ = err;
    s->dea = U16xI16_RL16(s->dea, 65536-p->alpha);
    return 1;
}